#define ZCORE_SOURCE
#include "MinpackSolver.hpp"
#ifdef ZZZ_LIB_MINPACK
#include <minpack.h>
#include <zMat.hpp>
#include "../../Utility/StringPrintf.hpp"

namespace zzz{

NonlinearSystem *mysys=NULL;

const string PrintResidue(const int *m, const double *fvec)
{
  double v=0;
  for (int i=0; i<*m; i++) v+=fvec[i]*fvec[i];
  return StringPrintf("Residue: %lf[%lf]", v, v/(*m));
}

void lmder_helper(const int *m, const int *n, const double *x, double *fvec, double *fjac, const int *ldfjac, int *iflag)
{
  if (*iflag==1)
  {
    mysys->CalculateResidue(fvec, x);
    ZLOG(ZDEBUG)<<PrintResidue(m,fvec);
  }
  else if (*iflag==2)
  {
    memset(fjac, 0,sizeof(double)*(*m)*(*n));
    zMatrixRawArrayDressW<double,zColMajor> jac=Dress(fjac, mysys->nConstraints_, mysys->nVar_);
    mysys->CalculateJacobian(jac, x);
  }
}

void InterpretError(int err)
{
  switch(err)
  {
  case 0: ZLOG(ZVERBOSE)<<"improper input parameters.\n"; break;

  case 1: ZLOG(ZVERBOSE)<<"algorithm estimates that the relative error in the sum of squares is at most tol.\n"; break;

  case 2: ZLOG(ZVERBOSE)<<"algorithm estimates that the relative error between x and the solution is at most tol.\n"; break;

  case 3: ZLOG(ZVERBOSE)<<"conditions for info = 1 and info = 2 both hold.\n"; break;

  case 4: ZLOG(ZVERBOSE)<<"fvec is orthogonal to the columns of the Jacobian to machine precision.\n"; break;

  case 5: ZLOG(ZVERBOSE)<<"number of calls to fcn has reached or exceeded 200*(n+1).\n"; break;

  case 6: ZLOG(ZVERBOSE)<<"tol is too small. no further reduction in the sum of squares is possible.\n"; break;

  case 7: ZLOG(ZVERBOSE)<<"tol is too small. no further improvement in the approximate solution x is possible.\n"; break;
  }
}
int MinpackSolver::Solve(NonlinearSystem &sys)
{
  //ATTENTION: this is NOT thread-safe
  mysys=&sys;
  zVector<double> init_value;
  sys.Initialize(init_value);

  ZCHECK_EQ(init_value.Size(0), sys.nVar_)
    <<"The number of initial value is not enough! Need: "
    <<sys.nVar_<<", Provided:"<<init_value.Size(0);
  int m=sys.nConstraints_;
  int n=sys.nVar_;
  zVector<double> fvec(m);  //output
  zMatrix<double> fjac(m, n);  //output
  int ldfjac=m;
  double tol=EPSILON;
  int info;          //output
  zVector<int> ipvt(n);    //output
  int lwa=m*n+5*n+m;
  zVector<double> wa(lwa);

  lmder1_(lmder_helper, &m, &n,init_value.Data(),fvec.Data(),fjac.Data(), &ldfjac, &tol, &info,ipvt.Data(),wa.Data(), &lwa);

  sys.WriteBack(init_value);

  InterpretError(info);

  return info;
}

}
#endif // ZZZ_LIB_MINPACK
